
/********require define*********/
#pragma REQUIRE_DEFINE
typedef struct OCTET_STRING
{
	uint8_t *buf; /* Buffer with consecutive OCTET_STRING bits */
	int size;	  /* Size of the buffer */

	asn_struct_ctx_t _asn_ctx; /* Parsing across buffer boundaries */
} OCTET_STRING_t;

/* MessageFrame */
typedef struct MessageFrame
{
	MessageFrame_PR present;
	union MessageFrame_u
	{
		BasicSafetyMessage_t bsmFrame;
		MapData_t mapFrame;
		RoadsideSafetyMessage_t rsmFrame;
		SPAT_t spatFrame;
		RoadSideInformation_t rsiFrame;
		/*
		 * This type is extensible,
		 * possible extensions are below.
		 */
	} choice;

	/* Context for parsing across buffer boundaries */
	asn_struct_ctx_t _asn_ctx;
} MessageFrame_t;
/* DSecond */
typedef long DSecond_t;
/* ParticipantData */
typedef struct ParticipantData
{
	ParticipantType_t ptcType;
	long ptcId;
	SourceType_t source;
	OCTET_STRING_t *id /* OPTIONAL */;
	DSecond_t secMark;
	PositionOffsetLLV_t pos;
	PositionConfidenceSet_t posConfidence;
	TransmissionState_t *transmission /* OPTIONAL */;
	Speed_t speed;
	Heading_t heading;
	SteeringWheelAngle_t *angle /* OPTIONAL */;
	struct MotionConfidenceSet *motionCfd /* OPTIONAL */;
	struct AccelerationSet4Way *accelSet /* OPTIONAL */;
	VehicleSize_t size;
	struct VehicleClassification *vehicleClass /* OPTIONAL */;
	/*
	 * This type is extensible,
	 * possible extensions are below.
	 */

	/* Context for parsing across buffer boundaries */
	asn_struct_ctx_t _asn_ctx;
} ParticipantData_t;

#define A_SET_OF(type)                   \
	struct                               \
	{                                    \
		type **array;                    \
		int count; /* Meaningful size */ \
		int size;  /* Allocated size */  \
		void (*free)(type *);            \
	}
#define A_SEQUENCE_OF(type) A_SET_OF(type)
/* ParticipantList */
typedef struct ParticipantList
{
	A_SEQUENCE_OF(struct ParticipantData)
	list;

	/* Context for parsing across buffer boundaries */
	asn_struct_ctx_t _asn_ctx;
} ParticipantList_t;

/* RoadsideSafetyMessage */
typedef struct RoadsideSafetyMessage
{
	MsgCount_t msgCnt;
	OCTET_STRING_t id;
	Position3D_t refPos;
	ParticipantList_t participants;
	/*
	 * This type is extensible,
	 * possible extensions are below.
	 */

	/* Context for parsing across buffer boundaries */
	asn_struct_ctx_t _asn_ctx;
} RoadsideSafetyMessage_t;
#pragma REQUIRE_REGION
/********end of require define*********/
#include <cmath>
#include <list>
// #include <ctime>
#include <pthread.h>
#include <unistd.h>
#include <mutex>
#if defined(__linux__)
#include <sys/timeb.h>

#endif

#define MAX_SIZE 64
#define TIMEOUT 100

//传入设成全局变量，就没有传入了
MessageFrame_t* mf = (MessageFrame_t*)data;

using namespace std;

//linux下使用usleep表示微妙
void usleep(unsigned long usec);

list<RoadsideSafetyMessage_t> road_msgs;
list<RoadsideSafetyMessage_t> vehi_msgs;

/**
 * @brief 融合路侧，车端的障碍物列表。消息时间戳与系统时间戳之差的绝对值不超过${TIMEOUT}ms
 * 
 * @param road_msg 路侧消息
 * @param vehi_msg 车端消息
 * @return RoadsideSafetyMessage_t 返回融合后的障碍物列表
 */
RoadsideSafetyMessage_t Merge(RoadsideSafetyMessage_t &road_msg, RoadsideSafetyMessage_t &vehi_msg);

/**
 * @brief 把融合后的消息通过socket发送出去
 * 
 * @param rsm 融合的障碍物列表
 * @return int 发送成功返回消息长度，发送失败返回-1
 */
int send_rsm_message(RoadsideSafetyMessage_t &rsm);

/**
 * @brief Get the now time 当前分钟的毫秒数
 * 
 * @return long 返回当前系统时间当前分钟的毫秒数
 */
long get_now_time()
{
	timeb now;
	ftime(&now);
	return ((now.time % 60)*10000 + now.millitm);
}

struct thread_data {
	RoadsideSafetyMessage_t* road_rsm;//路端的rsm消息
	RoadsideSafetyMessage_t* car_rsm;//车端的rsm消息
};
bool is_match(long timestamp, long now)
{
	return abs(timestamp - now) < TIMEOUT;
}

/**
 * @brief 定时调用，或其它事件触发调用。最好不要在接收消息时调用
 * 
 */
void find_matching()
{
	auto r = road_msgs.begin();
	auto v = vehi_msgs.begin();
	auto now = get_now_time();
	mutex a;
	while (r != road_msgs.end() || v != vehi_msgs.end())
	{
		if (is_match(v->participants.list.array[0]->secMark, now) &&
			is_match(r->participants.list.array[0]->secMark, now))
		{
			ock_gurad<mutex> lguard(m);
			auto res= Merge(*r,*v);
			send_rsm_message(res);
			road_msgs.erase(r);
			vehi_msgs.erase(v);
			
		}
		else if (v->participants.list.array[0]->secMark < r->participants.list.array[0]->secMark)
		{
			v++;
		}
		else
		{
			r++;
		}
	}
}

void network_cb_function_for_test(void *data)
{
	mutex m, n;
	// MessageFrame_t *obstacle_list = cheduangengxin();
	// RoadsideSafetyMessage_t *rsm = (RoadsideSafetyMessage_t *)malloc(sizeof(RoadsideSafetyMessage_t));

	if (mf->present == MessageFrame_PR_rsmFrame)
	{
		if (*(mf->choice.rsmFrame.id.buf) == 0x52) //0x52='R'
		{
			road_msgs.push_back(mf->choice.rsmFrame);
			while (road_msgs.size() > MAX_SIZE)
			{
				lock_gurad<mutex> lguard(m); 
				road_msgs.pop_front();
			}
		}
		else if (*(mf->choice.rsmFrame.id.buf) == 0x56) //0x56='V'
		{
			vehi_msgs.push_back(mf->choice.rsmFrame);
			while (vehi_msgs.size() > MAX_SIZE)
			{
				lock_gurad<mutex> lguard(n);
				vehi_msgs.pop_front();
			}
		}
	}
}
int main() {
	//定义两个线程，一个线程用于去感知融合，一个线程用于去接收车端和路侧的消息
	pthread_t threads[1];
	v2x_sdk_code_t ret = v2x_sdk_init();
	//v2x_sdk_code_t ret=v2x_sdk_init_max_rate();
	if (ret == v2x_OK) {
		log_print(LOG_INFO, "v2x_sdk_init finished!");
	}
	else {
		log_print(LOG_ERROR, "v2x_sdk_init error!");
	}
	dsm_req_info_t dsm_req;
	dsm_req.action = DSM_SERVICE_TYPE_ADD;
	aid_info_t aidinfo;
	aidinfo.aid = 17;
	//创建一个线程用于感知融合
	pthread_create(&threads[0], NULL, find_matching, NULL);  
	aidinfo.dsm_onrcv = network_cb_function_for_test;
	aidinfo.messageSetEnable = BSM_ENABLE;
	dsm_req.aidinfo = aidinfo;
	v2x_dsm_service_req(dsm_req);
	v2x_message_frame_t* snd_data = (v2x_message_frame_t*)malloc(sizeof(v2x_message_frame_t));
	snd_data->frame_choice = MF_BSM;
	MessageFrame_t* mf_snd = basicInitial_forTest();
	len_buf_t* data = object2asn(mf_snd);
	access_layer_tx_data_t* tx_data = (access_layer_tx_data_t*)malloc(sizeof(access_layer_tx_data_t));
	tx_data->choice = BSM_ENABLE;
	tx_data->data = data->buf;
	tx_data->data_len = data->len;

	printf("tx_data len : %d \n", data->len);
	while (1) {
		usleep(1 * millsec);

		v2x_sdk_network_send_data(tx_data);
	}
	pthread_join(threads[0],NULL);
}